#include "common_lib/speed_plan.h"

using namespace common_lib;

void TrapezoidalSpeedPlan::TrapezoidalCalcSpeed(amr_trajectory_msgs::amr_trajectory_msg &trajectory, float acc, float dcc, float vs, float &ve, float &vmax, int sign)
{
    if (trajectory.points.empty())
        return;
    float move_dis = trajectory.points.back().distance - trajectory.points[0].distance;
    float vlim = sqrt((2.0f*acc*dcc*move_dis+dcc*vs*vs-acc*ve*ve)/(dcc-acc));

    uint8_t plancase = 0;
    float t1 = 0.0f, t2 = 0.0f, t3 = 0.0f;
    float l1 = 0.0f, l2 = 0.0f, l3 = 0.0f;
    if ((vlim-vmax) > FLT_EPSILON)
    {
        vlim = vmax;
        t1 = fabs((vmax - vs) / acc);
        t2 = fabs((move_dis-0.5f*(vmax*vmax-vs*vs)/acc-0.5f*(ve*ve-vmax*vmax)/dcc)/vmax);
        t3 = fabs((ve-vmax)/dcc);
    }
    else
    {
        if (vlim>vs && vlim<ve)
        {
            ve = sqrt(vs*vs+2.0f*acc*move_dis);
            t1 = (ve-vs)/acc;
        }
        else if (vlim>ve && vlim<vs)
        {
            ve = sqrt(vs*vs+2.0f*dcc*move_dis);
            t3 = (ve-vs)/dcc;
        }
        else if (vlim>vs && vlim>ve)
        {
            t1 = (vlim-vs)/acc;
            t3 = (vlim-ve)/dcc;
        }
    }

    l1 = trajectory.points[0].distance + vs*t1+0.5f*acc*t1*t1;
    l2 = vlim * t2 + l1;
    l3 = ve*t3 - 0.5f*dcc*t3*t3+l2;

    // printf("t1: %f, t2: %f, t3: %f, l1: %f, l2: %f, l3: %f, %f\n", t1, t2, t3, l1, l2, l3, vlim);

    for (std::vector<amr_trajectory_msgs::point_info>::iterator iter = trajectory.points.begin();iter != trajectory.points.end();iter++)
    {
        if (iter->distance>=trajectory.points[0].distance && iter->distance<l1)
        {
            float ds = iter->distance - trajectory.points[0].distance;
            iter->target_speed = sign * sqrt(2.0f*acc*ds+vs*vs);
        }
        else if (iter->distance>=l1 && iter->distance<l2)
        {
            iter->target_speed = sign * vlim;
        }
        else if (iter->distance>=l2 && iter->distance<=l3)
        {
            float ds = l3 - iter->distance;
            iter->target_speed = sign * sqrt(ve*ve-2.0f*dcc*ds);
        }
    }
}

void TrapezoidalSpeedPlan::TrapezoidalCalcSpeed(std::vector<amr_trajectory_msgs::point_info> &points, int start, int end, float acc, float dcc, float vs, float &ve, float &vmax, int sign)
{
    if (points.empty())
        return;
    float move_dis = points[end].distance - points[start].distance;
    float vlim = sqrt((2.0f*acc*dcc*move_dis+dcc*vs*vs-acc*ve*ve)/(dcc-acc));

    uint8_t plancase = 0;
    float t1 = 0.0f, t2 = 0.0f, t3 = 0.0f;
    float l1 = 0.0f, l2 = 0.0f, l3 = 0.0f;
    if ((vlim-vmax) > FLT_EPSILON)
    {
        vlim = vmax;
        t1 = fabs((vmax - vs) / acc);
        t2 = fabs((move_dis-0.5f*(vmax*vmax-vs*vs)/acc-0.5f*(ve*ve-vmax*vmax)/dcc)/vmax);
        t3 = fabs((ve-vmax)/dcc);
    }
    else
    {
        if (vlim>vs && vlim<ve)
        {
            ve = sqrt(vs*vs+2.0f*acc*move_dis);
            t1 = (ve-vs)/acc;
        }
        else if (vlim>ve && vlim<vs)
        {
            ve = sqrt(vs*vs+2.0f*dcc*move_dis);
            t3 = (ve-vs)/dcc;
        }
        else if (vlim>vs && vlim>ve)
        {
            t1 = (vlim-vs)/acc;
            t3 = (vlim-ve)/dcc;
        }
    }

    l1 = points[start].distance + vs*t1+0.5f*acc*t1*t1;
    l2 = vlim * t2 + l1;
    l3 = ve*t3 - 0.5f*dcc*t3*t3+l2;

    // printf("t1: %f, t2: %f, t3: %f, l1: %f, l2: %f, l3: %f, %f, s: %d, e: %d\n", t1, t2, t3, l1, l2, l3, vlim, start, end);

    for (int i=start;i<=end;i++)
    {
        if (points[i].distance>=points[start].distance && points[i].distance<l1)
        {
            float ds = points[i].distance - points[start].distance;
            points[i].target_speed = sign * (2.0f*acc*ds+vs*vs);
        }
        else if (points[i].distance>=l1 && points[i].distance<l2)
        {
            points[i].target_speed = sign * vlim;
        }
        else if (points[i].distance>=l2 && points[i].distance-l3<=FLT_EPSILON)
        {
            float ds = l3 - points[i].distance;
            if (fabs(ds) < FLT_EPSILON)
                ds = 0.0f;
            points[i].target_speed = sign * sqrt(ve*ve-2.0f*dcc*ds);
        }
        // printf("%d --> speed: %f, x: %f, y: %f, distance: %f\n", i, points[i].target_speed, points[i].pose.position.x, points[i].pose.position.y, points[i].distance);
    }
}
